//
//  comstruct.c
//  OpensslTest
//
//  Created by jiahua on 16/3/29.
//  Copyright © 2016年 secsmarts. All rights reserved.
//
#include "sstComstruct.h"
#include "Define.h"
/* Return the tick count in ms */
int XTickCount(void)
{
    int currentTime;
#ifdef X_PLATFORM_WIN
    currentTime = GetTickCount();
#elif X_PLATFORM_IPHONE
    struct timeval current;
    gettimeofday(&current, NULL);
    currentTime = current.tv_sec * 1000 + current.tv_usec/1000;
#endif
    
#ifdef OS_VXWORKS
    ULONGA timeSecond = tickGet() / sysClkRateGet();
    ULONGA timeMilsec = tickGet() % sysClkRateGet() * 1000 / sysClkRateGet();
    currentTime = timeSecond * 1000 + timeMilsec;
#endif
    return currentTime;
}

int checkError(const u8 *checkBuf)
{
    int iRet = SST_OK;
    u8 error[5] = {0x45, 0x52, 0x52, 0x4F, 0x52};
    size_t size = 5;
    
    if(memcmp((const void *)error, (const void *)checkBuf, size) == 0)
    {
        iRet = SST_DEVICE_DECODE_ERROR;
    }
    
    return iRet;
}


unsigned int arc4random(void)
{
    unsigned int res = XTickCount();
    static unsigned int seed = 0xDEADB00B;
    
    seed = ((seed & 0x007F00FF) << 7) ^
    ((seed & 0x0F80FF00) >> 8) ^ // be sure to stir those low bits
    (res << 13) ^ (res >> 9);    // using the clock too!
    
    return seed;
}

void get_random_bytes(void *buf, size_t len)
{
    unsigned int ranbuf;
    unsigned int *lp;
    size_t i, count;
    count = len / sizeof(unsigned int);
    lp = (unsigned int *) buf;
    
    for(i = 0; i < count; i ++) {
        lp[i] = arc4random();
        len -= sizeof(unsigned int);
    }
    
    if(len > 0) {
        ranbuf = arc4random();
        memcpy((void *)&lp[i], (const void *)&ranbuf, len);
    }
}

int GetHash(u8* inData, int inDataLen, u8 *outHash)
{
    SHA256(inData, inDataLen, outHash);
    return 0;
}
int CharToByte(const u8 *inBuf, int inLen, u8 *outBuf)
{
    if(inLen%2 != 0)
    {
        return -1;
    }
    
    for(int i = 0; i < inLen; i += 2)
    {
        char temp = 0;
        
        if(inBuf[i] >= 'A' && inBuf[i] <= 'Z')
        {
            temp = inBuf[i] - 'A' + 10;
        }
        else if(inBuf[i] >= 'a' && inBuf[i] <= 'z')
        {
            temp = inBuf[i] - 'a' + 10;
        }
        else if(inBuf[i] >= '0' && inBuf[i] <= '9')
        {
            temp = inBuf[i] - '0';
        }
        else
        {
            temp = 0;
        }
        
        if(inBuf[i+1] >= 'A' && inBuf[i+1] <= 'Z')
        {
            temp =(temp << 4) |  inBuf[i+1] - 'A' + 10;
        }
        else if(inBuf[i+1] >= 'a' && inBuf[i+1] <= 'z')
        {
            temp = (temp << 4) | inBuf[i+1] - 'a' + 10;
        }
        else if(inBuf[i+1] >= '0' && inBuf[i+1] <= '9')
        {
            temp = (temp << 4) | inBuf[i+1] - '0';
        }
        else
        {
            temp = (temp << 4) |  0;
        }
        
        outBuf[i/2] = temp;
    }
    
    return 0;
}

void LogHex(const char *inLable, const u8 *inData, int inDataLen)
{
    int i;
    
    printf("\n\nchar %s[] = \n{\n\t", inLable);
    for(i = 0; i < inDataLen; i ++)
    {
        if(i%16 == 0)
        {
            printf("\n\t");
        }
        printf("0x%02X, ", inData[i]&0xff);
    }
    printf("\n};\n");
}


int getHelloToDevicePackage(u8 *inData, int inDataLen, u8 *inKey, int inKeyLen, u8 *outData, int *outDataLen)
{
    unsigned char Iv[16] = {0};
    SstSecHead* Sp = (SstSecHead*)outData;
    
    Sp->Token = SST_SEC_TOKEN;
    //Sp->Len = EncryptLen-sizeof(Sp->Cnt);
#ifdef _RSA_SYSTEM_
    Sp->Ver = SST_VERSION_RSA;
#else   //_RSA_SYSTEM_
    Sp->Ver = SST_VERSION_SM2;
#endif  //_RSA_SYSTEM_
    Sp->Cmd = SST_CMD_REQUEST;
    Sp->Token = htons(Sp->Token);
    Sp->Cnt = htons(0);
    //printf("update session information \n");
    Sp->Len = inDataLen;
    if(inKey == 0)//token, 连接上服务器的情况(R3 对K1 加密)
    {
        Sp->Len = htons(Sp->Len);
        memcpy(outData + SST_HEAD_SIZE, inData, inDataLen);//inDataLen = 64
        *outDataLen = inDataLen + SST_HEAD_SIZE;
     }
    else//ssid // 用默认R3 对 K1加密，K1 是自己产生的。
    {
        Sp->Len += HASH_DIGEST_LEN;
        Sp->Len = htons(Sp->Len);

        LogHex("R3", inKey, inKeyLen);
        LogHex("inData", inData, inDataLen);
        
        //CharToByte(inKey, inKeyLen * 2, initKeyFinal);
        AES_KEY key;
        AES_set_encrypt_key(inKey, SYM_KEY_LEN*8, &key);
        //        LogHex("inKey", initKeyFinal, 32);
        //        LogHex("outData", outData, SST_HEAD_SIZE+EncryptLen-sizeof(Sp->Cnt));
        
        AES_cbc_encrypt(inData, outData + SST_HEAD_SIZE, inDataLen, &key, Iv, AES_ENCRYPT);//inData = K1
        GetHash(inData, inDataLen, outData + SST_HEAD_SIZE + inDataLen);
        *outDataLen = SST_HEAD_SIZE+inDataLen+HASH_DIGEST_LEN;
    }
    return SST_OK;
}


int getDeviceAnswer(const u8 *inData, int inDataLen, u8 *inKey, int inKeyLen, u8 *outData, int *outDataLen, int type)
{
    int iRet = SST_OK;  //函数运行返回，如果正常运行，则返回0；否则返回值小于0
    int dataLen = 0;
    SstSecHead* Sp = (SstSecHead*)inData;
    u8 hashData[HASH_DIGEST_LEN] = {0};
    u16 SpToken;
    u16 SpLen;
    u8 Iv[SYM_BLOCK_LEN] = {0};
    
    if(inData == 0 || inDataLen ==0 || outData == NULL)                      //如果输入参数不合法
    {
        printf("\nDestructSecPacket, inData == 0 || inDataLen ==0 || outData == NULL");
        iRet = -311;
        goto END;
    }
    
    SpToken = ntohs(Sp->Token);
    SpLen = ntohs(Sp->Len);
    
    *outDataLen = 0;
    
    if(SpToken != SST_SEC_TOKEN || SpLen < 0 || SpLen > SST_SEC_SIZE)
    {
        printf("\nDestructSecPacket ERROR_PACKET_DAMAGE: Sp->Token=%d, Sp->Len=%d", SpToken, SpLen);
        iRet = SST_HANDSHAKE_ERROR;
        goto END;
    }
    else if (Sp->Cmd == CMD_ERROR)//广播或者错误
    {
        *outDataLen = inDataLen - SST_HEAD_SIZE - UDP_KEY_ID_LEN;
        memcpy(outData, inData + SST_HEAD_SIZE,  *outDataLen);
        return checkError(outData);
    }
    else
    {
        dataLen = SpLen - HASH_DIGEST_LEN;
        if (type == SST_CMD_UDP_DATA)
           dataLen -= UDP_KEY_ID_LEN;
        
        LogHex("inKey", inKey, inKeyLen);
        //CharToByte((const u8 *)inKey, inKeyLen * 2, initKeyFinal);
        LogHex("before decode", inData, inDataLen);

        AES_KEY key;
        AES_set_decrypt_key(inKey, SYM_KEY_LEN * 8, &key);
        AES_cbc_encrypt(inData + SST_HEAD_SIZE, outData, dataLen, &key, Iv, AES_DECRYPT);
        LogHex("after", inData, inDataLen);
        //memcpy(&(Sp->Cnt), tmpData, EncryptLen);
        GetHash(outData, dataLen, hashData);
        const u8 * peerHash = inData + SST_HEAD_SIZE + dataLen;
        if(memcmp((const void *)peerHash, (const void *)hashData, (size_t)HASH_DIGEST_LEN) != 0)
        {
            printf("\nDestructSecPacket Hash Error");
            iRet = -33;
            *outDataLen = 0;
            goto END;
        }
        else
        {
            iRet = 0;
            *outDataLen = dataLen;
        }
        // check error
        iRet = checkError(outData);
    }
END:
    return iRet;
}

int myConstructSecPacketRaw(u8 *inData, int inDataLen, u8 *inKey, int inKeyLen, int *inCnt, u8 SecTrans, u8 cmdTag, u8 *outData, int *outDataLen)
{
    int RemainLen = 0;
    int EncryptLen = 0;
    SstSecHead* Sp = (SstSecHead*)outData;
    u8 HashData[HASH_DIGEST_LEN];
    u8 Iv[16] = {0};
    //char cc[16];
    if(inData == 0 || inDataLen <= 0  || inDataLen > SST_CONT_SIZE|| outData == 0) //如果输入参数不合法
    {
        printf("\nConstructSecPacket, inData=%s, inDataLen=%d, outData = 0", inData, inDataLen);
        return SST_CMD_NO_SEC;
    }
    if(SecTrans == 0)
    {
        printf("\n###point at (%s)%d###\n",__FUNCTION__, __LINE__);
        
        Sp->Token = SST_SEC_TOKEN;
        //Sp->Len = EncryptLen-sizeof(Sp->Cnt);
#ifdef _RSA_SYSTEM_
        Sp->Ver = SST_VERSION_RSA;
#else   //_RSA_SYSTEM_
        Sp->Ver = SST_VERSION_SM2;
#endif  //_RSA_SYSTEM_
        Sp->Cmd = SST_CMD_NO_SEC;
        Sp->Cnt = ++(*inCnt);
        Sp->Token = htons(Sp->Token);
        Sp->Cnt = htons(Sp->Cnt);
        //printf("update session information \n");
        
        memcpy(outData+SST_HEAD_SIZE, inData,  inDataLen);
        *outDataLen = inDataLen+SST_HEAD_SIZE;
        
        Sp->Len = inDataLen;
        Sp->Len = htons(Sp->Len);
        
        return SST_OK;
    }
    else
    {
        memcpy(outData+SST_HEAD_SIZE, inData, inDataLen);
        //        printf("sizeof---->%d", sizeof(Sp->Cnt));
        EncryptLen = inDataLen+sizeof(Sp->Cnt);
        RemainLen = (SYM_BLOCK_LEN - EncryptLen%SYM_BLOCK_LEN)%SYM_BLOCK_LEN;
        if(RemainLen != 0)                               //如果不能成整块
        {
            EncryptLen += RemainLen;
            get_random_bytes(outData+SST_HEAD_SIZE+inDataLen, RemainLen);
        }
        
        Sp->Token = SST_SEC_TOKEN;
#ifdef _RSA_SYSTEM_
        Sp->Ver = SST_VERSION_RSA;
#else   //_RSA_SYSTEM_
        Sp->Ver = SST_VERSION_SM2;
#endif  //_RSA_SYSTEM_
        Sp->Cmd = ((RemainLen<<4)&0xF0) | (cmdTag & 0x0F);
       // printf("Sp->Cmd = %d\n", Sp->Cmd);
        Sp->Cnt = ++(*inCnt);//*inCnt的初始化为－1，加1后为0，后面的逻辑才好走
        //printf("Sp->Cnt = %d\n", Sp->Cnt);
        Sp->Len = EncryptLen-sizeof(Sp->Cnt)+HASH_DIGEST_LEN;
        
        Sp->Token = htons(Sp->Token);
        Sp->Cnt = htons(Sp->Cnt);
        Sp->Len = htons(Sp->Len);
        
        GetHash(outData, SST_HEAD_SIZE+EncryptLen-sizeof(Sp->Cnt), HashData);
        //LogHex("hashdata------> ", HashData, HASH_DIGEST_LEN);
        memcpy(outData+SST_HEAD_SIZE+EncryptLen-sizeof(Sp->Cnt), HashData, HASH_DIGEST_LEN);
        
        //EncryptLen += HASH_DIGEST_LEN;
        //LogHex("befor", outData, SST_HEAD_SIZE+EncryptLen-sizeof(Sp->Cnt)+HASH_DIGEST_LEN);
        AES_KEY key;
        AES_set_encrypt_key(inKey, SYM_KEY_LEN*8, &key);
        //LogHex("inKey", initKeyFinal, 32);
        //LogHex("outData", outData, SST_HEAD_SIZE+EncryptLen-sizeof(Sp->Cnt));
        AES_cbc_encrypt((u8*)(&(Sp->Cnt)), (u8*)(&(Sp->Cnt)), EncryptLen, &key, Iv, AES_ENCRYPT);
        //WritePos = (u8*)(&(Sp->Cnt));
        //memcpy(WritePos, tmpData,  EncryptLen);
        *outDataLen = SST_HEAD_SIZE + EncryptLen - sizeof(Sp->Cnt) + HASH_DIGEST_LEN;
        LogHex("after encrypt", outData, *outDataLen);
        //        LogHex("enprypt", outData, *outDataLen);
    }
    return SST_OK;
}


//判断包是否加密
// 1 表示是新家电，加密未知；0 表示旧家电，未加密
int isEncryptPackage (u8 *inData)
{
    return (inData[0]==(u8)0x83&&inData[1]==(u8)0x70);
}


int myDestructSecPacketRaw(u8 *inData, int inDataLen, u8 *inKey, int inKeyLen, int *inCnt, u8 *outData, int *outDataLen)
{
    int iRet = 0;  //函数运行返回，如果正常运行，则返回0；否则返回值小于0
    int EncryptLen = 0;
    int DataLen = 0;
    SstSecHead* Sp = (SstSecHead*)inData;
    u8 *tmpData = 0;
    u8 hashData[HASH_DIGEST_LEN] = {0};
    u16 SpToken;
    u16 SpLen;
    u16 SpCnt;
    u8 Iv[16] = {0};
    int offset = 0;
    
    if(inData == 0 || inDataLen ==0 || outData == 0)                      //如果输入参数不合法
    {
        printf("\nDestructSecPacket, inData == 0 || inDataLen ==0 || outData == 0");
        iRet = -311;
        goto END;
    }
    
    SpToken = ntohs(Sp->Token);
    SpLen = ntohs(Sp->Len);
    
    *outDataLen = 0;
    
    if(SpToken != SST_SEC_TOKEN || SpLen < 0 || SpLen > SST_SEC_SIZE)
    {
        printf("\nDestructSecPacket ERROR_PACKET_DAMAGE: Sp->Token=%d, Sp->Len=%d", SpToken, SpLen);
        iRet = SST_PACAKGE_ERROR;
        goto END;
    }
    else
    {
        if (Sp->Cmd != DISCUSS_CMD_RECV)
            offset = sizeof(Sp->Cnt);
            
        if(Sp->Cmd == CMD_ERROR)
        {
            memcpy(outData, inData + SST_HEAD_SIZE,  inDataLen - SST_HEAD_SIZE);
            *outDataLen = inDataLen - SST_HEAD_SIZE;
            (*inCnt) ++;
            goto END;
        }
        else
        {
            EncryptLen = SpLen + offset - HASH_DIGEST_LEN;
            DataLen = SpLen - HASH_DIGEST_LEN;
            if(EncryptLen <= 0 || EncryptLen % SYM_BLOCK_LEN != 0)
            {
                printf("\nDestructSecPacket EncryptLen SYM_BLOCK_LEN != 0");
                iRet = -321;
                goto END;
            }
            
            tmpData = (u8*)malloc(EncryptLen);
            if(tmpData == 0)
            {
                printf("\nDestructSecPacket, tmpData=%s", tmpData);
                iRet = -322;
                goto END;
            }
            LogHex("before decode", inData, inDataLen);
            AES_KEY key;
            AES_set_decrypt_key(inKey, SYM_KEY_LEN*8, &key);
            AES_cbc_encrypt((const u8 *)&(Sp->Cnt), (u8 *)&(Sp->Cnt), EncryptLen, &key, Iv, AES_DECRYPT);
            
            LogHex("after", inData, inDataLen);
            //memcpy(&(Sp->Cnt), tmpData, EncryptLen);
            GetHash(inData, SST_HEAD_SIZE + DataLen, hashData);
            if(memcmp(inData + SST_HEAD_SIZE + DataLen, hashData, HASH_DIGEST_LEN) != 0)
            {
                printf("\nDestructSecPacket Hash Error");
                iRet = SST_HASH_ERROR;
                goto END;
            }
            
            SpCnt = ntohs(Sp->Cnt);
            (*inCnt) ++;
            if((*inCnt) - 3 <= SpCnt && SpCnt <= (*inCnt) + 3)
            {
                *inCnt = SpCnt;
            }
            else
            {
                printf("\nDestructSecPacket, ReceivePacketCnt=%d, Sp->Cnt=%d", *inCnt, SpCnt);
                free(tmpData);
                iRet = -34;
                goto END;
            }
            
            memcpy(outData, inData+SST_HEAD_SIZE, DataLen-((Sp->Cmd>>4)&0x0F));
            *outDataLen = DataLen-((Sp->Cmd>>4)&0x0F);
            free(tmpData);
            
            iRet = 0;
            goto END;
        }
    }
    
END:
    if (iRet == SST_OK)
    {
        iRet = checkError(outData);
    }
    return iRet;
}

int isBroadCast(const u8* inData)
{
    int iRet = 0;  //函数运行返回，如果正常运行，则返回0；否则返回值小于0
   
    SstSecHead* Sp = (SstSecHead*)inData;
   
    if(inData == NULL)                      //如果输入参数不合法
    {
        iRet = SST_PACAKGE_ERROR;
        return iRet;
    }
    
   if (Sp->Cmd != CMD_ERROR)
       return 0;
    
   if (checkError(inData + SST_HEAD_SIZE) == SST_OK)
        return 1;
    
    return 1;
}
